/* ex: set tabstop=4 shiftwidth=4 expandtab: */

#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/types.h>
#include <pcap.h>

#include "base64.h"
#include "log.h"

#include "rtp_types.h"

typedef struct {
	u_char dst[6];
	u_char src[6];
	u_short type;
} ethernet_t;

typedef struct {
	u_char vhl;
	u_char tos;
	u_short len;
	u_short id;
	u_short off;
	u_char ttl;
	u_char p;
	u_short csum;
	struct in_addr src;
	struct in_addr dst;
} __attribute__ ((packed)) ip_t;

typedef struct {
	u_short sport;
	u_short dport;
	u_int seq;
	u_int ack;
	u_char offx2;
#define TH_OFF(th)	(((th)->offx2 & 0xF0) >> 4)
	u_char flags;
	u_short win;
	u_short csum;
	u_short urp;
} __attribute__ ((packed)) tcp_t;


static void
die(char *errbuf)
{
	printf("err: %s\n", errbuf);
	exit(1);
}

static u_short
__ntohs(u_short s)
{
	return (s >> 8) | (s << 8);
}

static u_long
__ntohl(u_long s)
{
	u_long ret;

	ret = s >> 24;
	ret |= (s & 0x00FF0000) >> 8;
	ret |= (s & 0x0000FF00) << 8;
	ret |= (s & 0x000000FF) << 24;

	return ret;
}


static void
lfx_print(char *s, int uplink)
{
	char buf[1024];
	char *p;
	int i, len, seq, t;
	char g[7] = {0};

	memset(buf, 0, 1024);

	/* remove last \r\n */
	len = strlen(s);
	if (s[len -1] == '\n') {
		s[len - 2] = '\0';
	}

	p = strstr(s, "g=");
	if (!p)
		return;
	snprintf(g, 6, "%s", p + 2);
	for (i = 0; i < 6; i++) {
		if (g[i] == '\r' || g[i] == '\n') {
			g[i] = '\0';
			break;
		}
	}

	p = strstr(s, "CSeq");
	if (!p)
		return;
	seq = atoi(p + 5);

	p = strstr(s, "t=");
	if (!p)
		return;
	t = atoi(p + 2);

	p = strstr(s, "l=");
	if (!p)
		return;
	len = atoi(p + 2);

	p = strstr(s, "d=");
	if (!p)
		return;
	p += 2;

	if (len > 0 && base64_decode(buf, p) == 0) {
		printf("base64 err[%s]\n", p);
	} else {
		printf("[%c:%d:g=%s:t=%d:l=%d]:\n", uplink ? 'C' : 'S', seq, g, t, len);
		hexdump((u_char *) buf, len);
	}
}

static void
__lfx_recv(char *s, int uplink, char *buf, int *offset)
{
	char *p;
	int len = 0;
	int hdrlen = 0;
	int cont_len = 0;

	len = strlen(s);
	memcpy(&buf[*offset], s, len);
	*offset += len;

	p = strstr(buf, "Content-Length");
	if (p)
		cont_len = atoi(p + 16);
	p = strstr(buf, "\r\n\r\n");
	if (p)
	   hdrlen = p - buf + 4;	

	if (*offset - hdrlen >= cont_len) {
		lfx_print(buf, uplink);
		memset(buf, 0, 1024);
		*offset = 0;
	}
}

static void
lfx_recv(char *s, int uplink)
{
	static char dl_buf[1024] = {0};
	static int dl_offset = 0;

	static char ul_buf[1024] = {0};
	static int ul_offset = 0;

	if (uplink) {
		__lfx_recv(s, 1, ul_buf, &ul_offset);
	} else {
		__lfx_recv(s, 0, dl_buf, &dl_offset);
	}
}

static FILE *avfp = NULL;
static int save_to_disk = 1;

static void
save_avdata(char *p, int len, int tag)
{
	int _len = len;
    int _tag = tag;

	if (save_to_disk) {
		fwrite(&_tag, sizeof(int), 1, avfp);
		fwrite(&_len, sizeof(int), 1, avfp);
		fwrite(p, len, 1, avfp);
	}
}

static void
parse_avdata(char *p, int len, int pkt)
{
	static int prev_ats = 0;
	static int prev_aframe = 0;
	static int prev_vts = 0;
	static int prev_vframe = 0;

	static int offset = 0;
	static int start = 0;
	static char buf[3000] = {0};

	rtp_channel_t *ch;
	rtp_hdr_t *rhdr;
	video_hdr_t *vhdr;
	audio_hdr_t *ahdr;
	char *payload;
	int ch_len = 0;

	memcpy(&buf[offset], p, len);
	offset += len;
	p = &buf[start];
	len = offset - start;

	do {
		if (len < sizeof(rtp_channel_t))
			goto reserve;

		ch = (void *) p;
		if (ch->tag != '$') {
			printf("tag fail %c\n", ch->tag);
			break;
		}

		ch_len = sizeof(rtp_channel_t) + __ntohs(ch->len);
		/* incomplete pkt */
		if (ch_len > len)
			goto reserve;

		rhdr = (void *) (p + sizeof(rtp_channel_t));
		printf("AV payload channel=%d pt=%d len=%d pkt=%d seq=%d ssrc=0x%08lX ts=%lu",
				ch->channel,
				rhdr->pt,
				__ntohs(ch->len),
				pkt,
			   	__ntohs(rhdr->seq),
				__ntohl(rhdr->ssrc), 
				__ntohl(rhdr->ts));

		payload = p + sizeof(rtp_channel_t) + sizeof(rtp_hdr_t);

		if (rhdr->pt == 96) {
            save_avdata(p, ch_len, 0xF0F0F0F0);
			vhdr = (void *) payload;
			if (vhdr->tag == 0xF0F0F0F0) {
				printf(" VIDEO=%d/%d/%d/%d\n",
						vhdr->len,
						vhdr->frame,
						(int) __ntohl(rhdr->ts) - prev_vts,
						vhdr->frame - prev_vframe);
				prev_vframe = vhdr->frame;
				prev_vts = __ntohl(rhdr->ts);
			} else {
				printf("\n");
			}
		} else if (rhdr->pt == 97) {
            save_avdata(p, ch_len, 0xE0E0E0E0);
			ahdr = (void *) payload;
			if (ahdr->tag == 0xE0E0E0E0) {
					printf(" AUDIO=%d/%d/%d/%d/%d\n",
							__ntohs(ahdr->au2.size),
							ahdr->len,
							ahdr->frame,
							(int) __ntohl(rhdr->ts) - prev_ats,
							ahdr->frame - prev_aframe);
					prev_aframe = ahdr->frame;
					prev_ats = __ntohl(rhdr->ts);
			} else {
				printf("\n");
			}
		} else {
			printf("\n");
		}


		p += ch_len;
		len -= ch_len;
		if (len <= 0)
			break;
		start += ch_len;
	} while (1);

	memset(buf, 0, sizeof(buf));
	offset = 0;
	start = 0;
	return;

	/* reserve len bytes from p on */
reserve:
	printf("reserve %d bytes\n", len);
	memmove(buf, p, len);
	start = 0;
	offset = len;
}

static void
parse(int pkt_idx, struct timeval *ts, const u_char *pkt, int len, int uplink, short port)
{
	static short avport = 0;

	int hdr;
	char s[2048], buf[1024];
	char *avp;
	int avlen;

	if (len < 4)
		return;

	hdr = *(int *) pkt;

	/* the first TCP channel is always the AV data channel */
	if (avport == 0)
		avport = port;

	/* GET POST HTTP */
	if (hdr == 0x20544547 ||
			hdr == 0x50545448 ||
			hdr == 0x54534F50)
		return;

	memset(s, 0, 2048);
	//snprintf(s, len, "%s", pkt);
	memcpy(s, pkt, len);

	printf("[%d] %lu.%lu\n", pkt_idx, ts->tv_sec, ts->tv_usec);
	/* RTSP */
	if (hdr == 0x50535452) {
		printf("[%c][%s]\n", uplink ? 'C' : 'S', s);
		/* AV DATA may be appended */
		if (len > 100) {
			char *p;

			p = strstr(s, "\r\n\r\n");
			if (p && p[4] == '$') {
				avp = &p[4];
				avlen = len - (avp - s);
				goto avdata;
			}
		}
		printf("*****************************\n");
		return;
	}

	/* LFX_MSG */
	if (hdr == 0x5F58464C) {
		lfx_recv(s, uplink);
		printf("*****************************\n");
		return;
	}

	if (avport == port || len > 1000) {
		if (!uplink) {
			avp = s;
			avlen = len;
			goto avdata;
		} else {
			printf("UNKNOWN DATA(%d)\n", len);
		}
		printf("*****************************\n");
		return;
	}

	memset(buf, 0, 1024);

	/* base64 */
	if (base64_decode(buf, s) == 0) {
		if (!uplink) {
			avp = s;
			avlen = len;
			goto avdata;
		} else {
			printf("UNKNOWN DATA(%d)\n", len);
		}
		printf("*****************************\n");
	} else {
		hdr = *(int *) buf;
		/* check for RTSP/DESCRIBE/SETUP/PLAY/TEARDOWN */
		if (hdr == 0x50535452 ||
				hdr == 0x43534544 ||
				hdr == 0x55544553 ||
				hdr == 0x59414C50 ||
				hdr == 0x52414554) {
			printf("[%c][%s]\n", uplink ? 'C' : 'S', buf);
			printf("*****************************\n");
			return;
		}
		lfx_recv(buf, uplink);
		printf("*****************************\n");
	}

	return;

avdata:
	parse_avdata(avp, avlen, pkt_idx);
#if 0
#define AVTAG "AVDT"
	fwrite(AVTAG, sizeof(AVTAG), 1, avfp);
	fwrite(&avlen, sizeof(avlen), 1, avfp);
	fwrite(avp, avlen, 1, avfp);
#endif
	printf("*****************************\n");
}

static void
pcap_cb(u_char *args, const struct pcap_pkthdr *hdr,
			     const u_char *pkt)
{
	static struct timeval starttime = {0, 0};
	static int pkt_idx = 0;

	tcp_t *tcp;
	ip_t *ip = (void *) (pkt + 14);
	int len;
	const u_char *payload;
	struct timeval ts;
	int flag = (int) args;
	int uplink;
	short port;

	pkt_idx++;

	if (ip->p != 6)
		return;

	len = __ntohs(ip->len);
	tcp = (void *) ((char *) ip + 20);

	len -= 20 + 4 * TH_OFF(tcp);
	if (len <= 0)
		return;
	payload = pkt + 14 + 20 + 4 * TH_OFF(tcp);

	ts = hdr->ts;

	if (starttime.tv_sec == 0 && starttime.tv_usec == 0) {
		starttime = ts;
		ts.tv_sec = 0;
		ts.tv_usec = 0;
	} else {
		ts.tv_sec -= starttime.tv_sec;
		if (ts.tv_usec < starttime.tv_usec) {
			ts.tv_sec--;
			ts.tv_usec += 1000000;
		}
		ts.tv_usec -= starttime.tv_usec;
	}

	uplink = (ip->src.s_addr == 0x3501A8C0); /* hardcoded IP addr of PSP */
	port = uplink ? __ntohs(tcp->sport) : __ntohs(tcp->dport);

	if (flag == 0)
		parse(pkt_idx, &ts, payload, len, uplink, port);
	else if (flag == 1) {
		if (uplink)
			parse(pkt_idx, &ts, payload, len, 1, port);
	} else {
		if (!uplink)
			parse(pkt_idx, &ts, payload, len, 0, port);
	}
}


int
main(int argc, char **argv)
{
	char errbuf[PCAP_ERRBUF_SIZE];
	pcap_t *fp;
	int flag = 0; /* 0 for ALL, 1 for UPLINK_ONLY, 2 for DOWNLINK_ONLY */

	base64_init();
	fp = pcap_open_offline(argv[1], errbuf);
	if (!fp)
		die(errbuf);
	if (argc > 2)
		flag = atoi(argv[2]);
	if (save_to_disk)
		avfp = fopen("avdata.raw", "wb");
	pcap_loop(fp, -1, pcap_cb, (void *) flag);
	pcap_close(fp);
	if (save_to_disk)
		fclose(avfp);

	return 0;
}
